#include <cstdlib>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <vector>
#include <cmath>
#include <limits>
#include <iostream>
#include <ros/ros.h>
#include <ros/package.h>

#include <opencv2/dnn.hpp>
#include <opencv2/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgcodecs/imgcodecs.hpp>

#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>

#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/PointCloud2.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <pcl/io/pcd_io.h>
#include <pcl/common/common.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/crop_box.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/passthrough.h>


using namespace std;

// calibration data for camera and lidar
static cv::Mat P_rect_00(3,4,cv::DataType<double>::type); // 3x4 projection matrix after rectification
static cv::Mat R_rect_00(4,4,cv::DataType<double>::type); // 3x3 rectifying rotation to make image planes co-planar
static cv::Mat RT(4,4,cv::DataType<double>::type); // rotation matrix and translation vector
static string yoloClassesFile = "/home/zh/lidarimg_ws/yolo/coco.names";
static string yoloModelConfiguration = "/home/zh/lidarimg_ws/yolo/yolov3.cfg";
static string yoloModelWeights = "/home/zh/lidarimg_ws/yolo/yolov3.weights";
static image_transport::Publisher pub_imgdetec;

struct BoundingBox {
    int boxID; 
    int trackID;
    
    cv::Rect roi; 
    int classID; 
    double confidence;
    std::vector<cv::KeyPoint> keypoints; // keypoints enclosed by 2D roi
    std::vector<cv::DMatch> kptMatches; // keypoint matches enclosed by 2D roi
};

void detectObjects(cv::Mat& img, std::vector<BoundingBox>& bBoxes, float confThreshold, float nmsThreshold, 
                std::string classesFile, std::string modelConfiguration, std::string modelWeights, bool bVis)
{
    
    // load class names from file
    vector<string> classes;
    ifstream ifs(classesFile.c_str());
    string line;
    while (getline(ifs, line)) classes.push_back(line);
    
    // 载入darknet
    cv::dnn::Net net = cv::dnn::readNetFromDarknet(modelConfiguration, modelWeights);
    
    net.setPreferableBackend(cv::dnn::DNN_BACKEND_OPENCV);
    // net.setPreferableTarget(cv::dnn::DNN_TARGET_CPU);
    // net.setPreferableTarget(cv::dnn::DNN_TARGET_OPENCL);
    
    // 4D blob
    cv::Mat blob;
    vector<cv::Mat> netOutput;
    double scalefactor = 1/255.0;
    // cv::Size size = cv::Size(416, 416);
    // cv::Size size = cv::Size(1024, 1024);
    // cv::Size size = cv::Size(608, 608);
    cv::Size size = cv::Size(320, 320);
    // cv::Size size = cv::Size(160, 160);
    cv::Scalar mean = cv::Scalar(0,0,0);
    bool swapRB = true;
    bool crop = false;
    
    blob = cv::dnn::blobFromImage(img,  scalefactor, size, mean, swapRB, crop);
    // cout << blob.flags << "*" << blob.isContinuous() << endl;
    
    vector<cv::String> names;
    vector<int> outLayers = net.getUnconnectedOutLayers();
    vector<cv::String> layersNames = net.getLayerNames();
    
    names.resize(outLayers.size());
    for (size_t i = 0; i < outLayers.size(); ++i){
        names[i] = layersNames[outLayers[i] - 1];
    }

    double t = (double)cv::getTickCount();
    net.setInput(blob);
    net.forward(netOutput, names);
    t = ((double)cv::getTickCount() - t) / cv::getTickFrequency();
    cout <<  1000 * t / 1.0 << " ms" << endl;

    
    // Scan through all bounding boxes and keep only the ones with high confidence
    vector<int> classIds; vector<float> confidences; vector<cv::Rect> boxes;
    for (size_t i = 0; i < netOutput.size(); ++i)
    {
        float* data = (float*)netOutput[i].data;
        for (int j = 0; j < netOutput[i].rows; ++j, data += netOutput[i].cols)
        {
            cv::Mat scores = netOutput[i].row(j).colRange(5, netOutput[i].cols);
            cv::Point classId;
            double confidence;
            
            // Get the value and location of the maximum score
            cv::minMaxLoc(scores, 0, &confidence, 0, &classId);
            if (confidence > confThreshold)
            {
                cv::Rect box; int cx, cy;
                cx = (int)(data[0] * img.cols);
                cy = (int)(data[1] * img.rows);
                box.width = (int)(data[2] * img.cols);
                box.height = (int)(data[3] * img.rows);
                box.x = cx - box.width/2; // left
                box.y = cy - box.height/2; // top
                
                boxes.push_back(box);
                classIds.push_back(classId.x);
                confidences.push_back((float)confidence);
            }
        }
    }
    
    // perform non-maxima suppression
    vector<int> indices;
    cv::dnn::NMSBoxes(boxes, confidences, confThreshold, nmsThreshold, indices);
    for(auto it=indices.begin(); it!=indices.end(); ++it) {
        
        BoundingBox bBox;
        bBox.roi = boxes[*it];
        bBox.classID = classIds[*it];
        bBox.confidence = confidences[*it];
        bBox.boxID = (int)bBoxes.size(); // zero-based unique identifier for this bounding box
        if(bBox.classID == 2){
            bBoxes.push_back(bBox);
        }
    }
    
    // show results
    if(bVis) {
        for(auto it=bBoxes.begin(); it!=bBoxes.end(); ++it) {
            
            // Draw rectangle displaying the bounding box
            int top, left, width, height;
            top = (*it).roi.y;
            left = (*it).roi.x;
            width = (*it).roi.width;
            height = (*it).roi.height;
            cv::rectangle(img, cv::Point(left, top), cv::Point(left+width, top+height),cv::Scalar(0, 255, 0), 2);
            
            string label = cv::format("%.2f", (*it).confidence);
            label = classes[((*it).classID)] + ":" + label;
        
            // Display label at the top of the bounding box
            int baseLine;
            cv::Size labelSize = getTextSize(label, cv::FONT_ITALIC, 0.5, 1, &baseLine);
            top = max(top, labelSize.height);
            rectangle(img, cv::Point(left, top - round(1.5*labelSize.height)), cv::Point(left + round(1.5*labelSize.width), top + baseLine), cv::Scalar(255, 255, 255), cv::FILLED);
            cv::putText(img, label, cv::Point(left, top), cv::FONT_ITALIC, 0.75, cv::Scalar(0,0,0),1);            
        }        
        string windowName = "车辆检测";
        cv::namedWindow( windowName, 3 );
        cv::imshow( windowName, img );
        cv::waitKey(1);
    }   
}

void showLidarImgOverlay(cv::Mat &img, pcl::PointCloud<pcl::PointXYZ>::Ptr lidarPoints)
{
    // find max. x-value
    double maxVal = 0.0; 
    for (size_t i = 0; i < lidarPoints->points.size(); ++i){
        maxVal = maxVal < lidarPoints->points[i].x ? lidarPoints->points[i].x : maxVal;
    }

    cv::Mat X(4,1,cv::DataType<double>::type);
    cv::Mat Y(3,1,cv::DataType<double>::type);
    for(size_t i = 0; i < lidarPoints->points.size(); ++i) {
        X.at<double>(0, 0) = lidarPoints->points[i].x;
        X.at<double>(1, 0) = lidarPoints->points[i].y;
        X.at<double>(2, 0) = lidarPoints->points[i].z;
        X.at<double>(3, 0) = 1;

        Y = P_rect_00 * R_rect_00 * RT * X;
        cv::Point pt;
        pt.x = Y.at<double>(0, 0) / Y.at<double>(0, 2);
        pt.y = Y.at<double>(1, 0) / Y.at<double>(0, 2);

        float val = lidarPoints->points[i].x;

        int red = min(255, (int)(255 * abs((val - maxVal) / maxVal)));
        int green = min(255, (int)(255 * (1 - abs((val - maxVal) / maxVal))));
        cv::circle(img, pt, 1.5, cv::Scalar(125, green, red), -1);
    }

    string windowName = "LiDAR on Image";
    cv::namedWindow( windowName, 2 );
    cv::imshow( windowName, img );
    cv::waitKey(1);
}
void fusionCallback(const sensor_msgs::ImageConstPtr& image, const sensor_msgs::PointCloud2ConstPtr& cloud)
{

    cout << "image_time: " << image->header.stamp << "\n"  << "lidar_time: " << cloud->header.stamp << "\n"  << endl;

    /* image process */
    cv_bridge::CvImagePtr cv_ptr;
    cv_ptr = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::BGR8);
    cv::Mat cvimg = cv_ptr->image;

    std::string windowName = "Image Raw";
    cv::namedWindow( windowName, 1 );
    cv::imshow( windowName, cvimg );
    cv::waitKey(1);

    /* lidar process */ 
    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*cloud, *pcl_cloud);
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud (pcl_cloud);
    pass.setFilterFieldName ("x");
    pass.setFilterLimits (0.0, 100);
    pass.filter (*pcl_cloud);


    pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr centerPoints(new pcl::PointCloud<pcl::PointXYZ>);
    // lidarCluster(pcl_cloud, cluster_cloud, centerPoints);

    cv::Mat visImg; 
    visImg = cvimg.clone();
    showLidarImgOverlay(visImg, pcl_cloud);

    float confThreshold = 0.2;
    float nmsThreshold = 0.4;
    bool bVis = true;

    std::vector<BoundingBox> bboxs;
    detectObjects(visImg, bboxs, confThreshold, nmsThreshold,
                    yoloClassesFile, yoloModelConfiguration, yoloModelWeights, bVis);

    sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", visImg).toImageMsg();
    pub_imgdetec.publish(msg);

}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "fusion");
    ros::NodeHandle nh;

    RT.at<double>(0,0) = 7.533745e-03; RT.at<double>(0,1) = -9.999714e-01; RT.at<double>(0,2) = -6.166020e-04; RT.at<double>(0,3) = -4.069766e-03;
    RT.at<double>(1,0) = 1.480249e-02; RT.at<double>(1,1) = 7.280733e-04; RT.at<double>(1,2) = -9.998902e-01; RT.at<double>(1,3) = -7.631618e-02;
    RT.at<double>(2,0) = 9.998621e-01; RT.at<double>(2,1) = 7.523790e-03; RT.at<double>(2,2) = 1.480755e-02; RT.at<double>(2,3) = -2.717806e-01;
    RT.at<double>(3,0) = 0.0; RT.at<double>(3,1) = 0.0; RT.at<double>(3,2) = 0.0; RT.at<double>(3,3) = 1.0;

    R_rect_00.at<double>(0,0) = 9.999239e-01; R_rect_00.at<double>(0,1) = 9.837760e-03; R_rect_00.at<double>(0,2) = -7.445048e-03; R_rect_00.at<double>(0,3) = 0.0;
    R_rect_00.at<double>(1,0) = -9.869795e-03; R_rect_00.at<double>(1,1) = 9.999421e-01; R_rect_00.at<double>(1,2) = -4.278459e-03; R_rect_00.at<double>(1,3) = 0.0;
    R_rect_00.at<double>(2,0) = 7.402527e-03; R_rect_00.at<double>(2,1) = 4.351614e-03; R_rect_00.at<double>(2,2) = 9.999631e-01; R_rect_00.at<double>(2,3) = 0.0;
    R_rect_00.at<double>(3,0) = 0; R_rect_00.at<double>(3,1) = 0; R_rect_00.at<double>(3,2) = 0; R_rect_00.at<double>(3,3) = 1;

    P_rect_00.at<double>(0,0) = 7.215377e+02; P_rect_00.at<double>(0,1) = 0.000000e+00; P_rect_00.at<double>(0,2) = 6.095593e+02; P_rect_00.at<double>(0,3) = 0.000000e+00;
    P_rect_00.at<double>(1,0) = 0.000000e+00; P_rect_00.at<double>(1,1) = 7.215377e+02; P_rect_00.at<double>(1,2) = 1.728540e+02; P_rect_00.at<double>(1,3) = 0.000000e+00;
    P_rect_00.at<double>(2,0) = 0.000000e+00; P_rect_00.at<double>(2,1) = 0.000000e+00; P_rect_00.at<double>(2,2) = 1.000000e+00; P_rect_00.at<double>(2,3) = 0.000000e+00; 


    message_filters::Subscriber<sensor_msgs::Image> cam_sub_;
    message_filters::Subscriber<sensor_msgs::PointCloud2> lidar_sub_;

    image_transport::ImageTransport it(nh);
    pub_imgdetec = it.advertise("camDetec",10);


    // message_filters::sync_policies::ExactTime
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::PointCloud2> cam_lidar_fuse_policy;
    typedef message_filters::Synchronizer<cam_lidar_fuse_policy> Sync;
    boost::shared_ptr<Sync> sync;

    string cam_topic_ = "/kitti/camera_color_left/image_raw";
    string lidar_topic_ = "/kitti/velo/pointcloud";
    // string lidar_topic_ = "cluster";


    // nh.getParam("cam_topic_", cam_topic_);
    // nh.getParam("lidar_topic_", lidar_topic_);
    // nh.getParam("yoloClassesFile", yoloClassesFile);
    // nh.getParam("yoloModelConfiguration", yoloModelConfiguration);
    // nh.getParam("yoloModelWeights", yoloModelWeights);

    cam_sub_.subscribe(nh, cam_topic_, 1);
    lidar_sub_.subscribe(nh, lidar_topic_, 1);

    sync.reset(new Sync(cam_lidar_fuse_policy(10), cam_sub_, lidar_sub_));
    sync->registerCallback(boost::bind(&fusionCallback, _1, _2));

    

    
    ros::spin();
}